#include "vehicle.h"


Vehicle::Vehicle()
{

    Realworld=imread("E:/QT_corder/SmartCar_fundenment/chepter6/P1000C1.bmp");
    imshow("RealWorld",Realworld);
    realxyhInit(2,2,0);
}

void Vehicle::realxyhInit(double mx,double my,double Degheading)
{
    realx=mx;
    realy=my;
    realheading=Degheading;
    realxyhInited=true;
    //update(0,0,0.02);
    senx=0;
    seny=0;
    senheading=0;
}

void Vehicle::update1(double nAng,double nspeed,double interval)
{
    //nAng=-180~180°,-180°为逆时针自转 0为直行，
    double tr;
    double tspdl;
    double tspdr;
    nAng=nAng>180?180:nAng;
    nAng=nAng<-180?-180:nAng;

    if(nAng<0.01&&nAng>-0.01)
        //update(nspeed*0.3,nspeed*0.3,interval);
        update(nspeed,nspeed,interval);
    else
    {
        if(nAng>=0.01)
        {
            tr=180/nAng*wheelbase/2-wheelbase/2;
        }
        else
        {
            tr=180/nAng*wheelbase/2+wheelbase/2;
        }
        tspdl=nspeed+nspeed*wheelbase/2/tr;
        tspdr=nspeed-nspeed*wheelbase/2/tr;
        update(tspdl*1.0,tspdr*1.0,interval);
        //update(tspdl,tspdr,interval);

    }
}

void Vehicle::update(double nTarspdl, double nTarspdr,double interval)
{
    double theta,centx,centy,trang;
    //本程序更新车辆在真实世界的位置
    //限定速度
    nTarspdl=nTarspdl>600?600:nTarspdl;
    nTarspdl=nTarspdl<-600?-600:nTarspdl;
    nTarspdr=nTarspdr>600?600:nTarspdr;
    nTarspdr=nTarspdr<-600?-600:nTarspdr;

    //增加速度变化梯度，这里给出目前应该的转速
    if(nTarspdl>crtSpdL)
    {
        //加速
         crtSpdL=nTarspdl>=(crtSpdL+Accl*60*interval)?(crtSpdL+Accl*60*interval):nTarspdl;
    }
    else
    {
        //减速
        crtSpdL=nTarspdl<=(crtSpdL-Accl*60*interval)?(crtSpdL-Accl*60*interval):nTarspdl;
    }

    if(nTarspdr>crtSpdR)
    {
        //加速
         crtSpdR=nTarspdr>=(crtSpdR+Accl*60*interval)?(crtSpdR+Accl*60*interval):nTarspdr;
    }
    else
    {
        //减速
        crtSpdR=nTarspdr<=(crtSpdR-Accl*60*interval)?(crtSpdR-Accl*60*interval):nTarspdr;
    }

    double tdist;   //车辆中心行驶距离
    spdl=crtSpdL/60*CV_PI*wheeldim;   //转速转换成m/s
    spdr=crtSpdR/60*CV_PI*wheeldim;   //rpm-m/s
    spd=(spdl+spdr)/2;

    if(abs(spdl-spdr)<0.0001)
    {
        //直线行驶
         tdist=spd*interval;
         realx=realx-tdist*sin(realheading/180*CV_PI);
         realy=realy+tdist*cos(realheading/180*CV_PI);
         centx=realx;
         centy=realy;

    }
    else
    {
        //圆弧行驶
        r=(spdl+spdr)/(spdl-spdr)*wheelbase/2;  //正则顺时针旋转

        if(abs(r)<0.0001)
        {
            realheading=realheading+spdl*interval/(wheelbase*CV_PI)*360;
        }
        else
        {

            if(spdl>spdr)
            {
                //clockwise
                trang=-spd*interval/(r*CV_PI)*360;
                theta=realheading+90;
                theta=theta>=360?theta-360:theta;
                centx=realx-r*sin(theta/180*CV_PI);
                centy=realy+r*cos(theta/180*CV_PI);
            }
            else
            {
                //counterclockwise
                trang=-spd*interval/(r*CV_PI)*360;
                theta=realheading-90;
                theta=theta<0?theta+360:theta;
                centx=realx+r*sin(theta/180*CV_PI);
                centy=realy-r*cos(theta/180*CV_PI);
            }
            realheading=realheading-trang;
            double tx,ty;

            Rotate2(realx-centx,realy-centy,-trang/180*CV_PI,tx,ty);
            realx=tx+centx;
            realy=ty+centy;

        }
        if (realheading>=360) realheading=realheading-360;
        if (realheading<0) realheading=realheading+360;

    }

    //qDebug()<<r<<centx<<centy<<realx<<realy<<realheading;
    double tx,ty;
    tx=realx-vehr*sin(realheading/180*CV_PI);
    ty=realy+vehr*cos(realheading/180*CV_PI);
    Mat p=Realworld.clone();
    //p=Realworld(Rect(0,0,Realworld.cols,Realworld.rows-1));
    //circle(p, Point(realx/sceneScale, realy/sceneScale), vehr/sceneScale,Scalar(0,0,255),2);
    circle(p, Point(realx/sceneScale, realy/sceneScale), 15.0,Scalar(0,0,255),2);
    line(p,Point(tx/sceneScale,ty/sceneScale),Point(realx/sceneScale,realy/sceneScale),Scalar(255,0,0));
    line(p,Point(centx/sceneScale,centy/sceneScale),Point(realx/sceneScale,realy/sceneScale),Scalar(255,255,0));


    //绘制路径
    if(path->size()>1)
    {
        for(int i=0;i<path->size()-1;i++)
        line(p,Point(path->at(i).x()/sceneScale,path->at(i).y()/sceneScale),Point(path->at(i+1).x()/sceneScale,path->at(i+1).y()/sceneScale),Scalar(0,0,255));
    }
    if(path->size()>1)
    {
        for(int i=0;i<path->size();i++)
            circle(p,Point(path->at(i).x()/sceneScale,path->at(i).y()/sceneScale),10,Scalar(0,255,0),2);
    }

    sen(crtSpdL,crtSpdR,interval);
    imshow("RealWorld",p);

}

void Vehicle::sen(double nspdl, double nspdr,double interval)
{
    double theta,centx,centy,trang;
    //本程序更新车辆在真实世界的位置
    //限定速度
    nspdl=nspdl>600?600:nspdl;
    nspdl=nspdl<-600?-600:nspdl;
    nspdr=nspdr>600?600:nspdr;
    nspdr=nspdr<-600?-600:nspdr;

    double tdist;   //车辆中心行驶距离
    spdl=nspdl/60*CV_PI*wheeldim;   //rpm-m/s
    spdr=nspdr/60*CV_PI*wheeldim;   //rpm-m/s
    spd=(spdl+spdr)/2;

    double rr;
    if(abs(spdl-spdr)<0.0001)
    {
        //直线行驶
         tdist=spd*interval;
         senx=senx-tdist*sin(senheading/180*CV_PI);
         seny=seny+tdist*cos(senheading/180*CV_PI);
         centx=senx;
         centy=senx;
    }
    else
    {
        //圆弧行驶
        rr=(spdl+spdr)/(spdl-spdr)*wheelbase/2;  //正则顺时针旋转

        if(abs(rr)<0.0001)
        {
            senheading=senheading+spdl*interval/(wheelbase*CV_PI)*360;
        }
        else
        {

            if(spdl>spdr)
            {
                //clockwise
                trang=-spd*interval/(rr*CV_PI)*360;
                theta=senheading+90;
                theta=theta>=360?theta-360:theta;
                centx=senx-rr*sin(theta/180*CV_PI);
                centy=seny+rr*cos(theta/180*CV_PI);
            }
            else
            {
                //counterclockwise
                trang=-spd*interval/(rr*CV_PI)*360;
                theta=senheading-90;
                theta=theta<0?theta+360:theta;
                centx=senx+rr*sin(theta/180*CV_PI);
                centy=seny-rr*cos(theta/180*CV_PI);
            }
            senheading=senheading-trang;
            double tx,ty;

            Rotate2(senx-centx,seny-centy,-trang/180*CV_PI,tx,ty);
            senx=tx+centx;
            seny=ty+centy;

        }
        if (senheading>=360) senheading=senheading-360;
        if (senheading<0) senheading=senheading+360;

    }

//    qDebug()<<r<<centx<<centy<<senx<<seny<<senheading<<realheading;
   // double tx,ty;
  //  tx=realx-vehr*sin(realheading/180*CV_PI);
  //  ty=realy+vehr*cos(realheading/180*CV_PI);

}

void Vehicle::Rotate2(double x1, double y1, double alpha, double& x2, double& y2)
{
    x2 = x1 * cos(alpha) - y1 * sin(alpha);
    y2 = x1 * sin(alpha) + y1 * cos(alpha);
}



